#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point, PointStamped, PoseWithCovarianceStamped, Quaternion, PoseStamped, PoseArray
from nav_msgs.msg import Path
from nav_msgs.srv import GetPlan, GetPlanRequest
from move_base_msgs.msg import MoveBaseActionGoal, MoveBaseAction, MoveBaseGoal, MoveBaseActionResult
from actionlib import SimpleActionClient, GoalStatus
from actionlib_msgs.msg import GoalID, GoalStatusArray
from PyQt5.QtCore import QTimer

STATUS_MAP = {
    "0": "待定", "1": "活动中", "2": "被中断", "3": "成功到达目标", "4": "放弃",
    "5": "拒绝", "6": "中断中", "7": "撤回中", "8": "已撤回", "9": "丢失",
}


class NavControl:
    m_target_point_maps_ = {}

    def __init__(self, plainTextEdit, append_row, set_stop_btn_disabled, set_start_btn_disabled):
        self.plainTextEdit = plainTextEdit
        self.append_row = append_row
        self.set_stop_btn_disabled = set_stop_btn_disabled
        self.set_start_btn_disabled = set_start_btn_disabled
        self.message = None
        rospy.init_node('marker_publisher', anonymous=True)

        self.m_global_path_pub_ = rospy.Publisher('/global_path', Path, queue_size=10)
        # self.l_path_pub = rospy.Publisher("/move_base_node/NavfnROS/plan", Path, queue_size=10)
        # self.m_goal_status_sub_ = rospy.Subscriber("/move_base/status", GoalStatusArray, self.goal_status_callback,
        #                                            queue_size=10)

        self.l_marker_pub_ = rospy.Publisher('visualization_marker', Marker, queue_size=1)

        # 订阅工具栏的Publish Point话题
        # self.l_clicked_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.handleClickedPoint)
        self.l_2d_nav_goal_sub = rospy.Subscriber("/move_base_simple/goal_temp", PoseStamped,
                                                  self.handle_2d_nav_goal_clicked)

        self.l_move_base_client = SimpleActionClient('move_base', MoveBaseAction)
        self.l_goal_pub_ = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10)
        self.l_goal_cancel_pub_ = rospy.Publisher('/move_base/cancel', GoalID, queue_size=1)
        # self.l_goal_status_sub = rospy.Subscriber("/move_base/status", GoalStatusArray, self.handle_goal_status)
        self.l_goal_result_sub = rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.handle_goal_result)

        self.last_message = u""

        self.task_timer = QTimer()
        self.task_timer.timeout.connect(self.message_thread)
        self.task_timer.start(100)

        plan_url = "/move_base_node/GlobalPlanner/make_plan"
        rospy.wait_for_service(plan_url)
        self.make_plan_client = rospy.ServiceProxy(plan_url, GetPlan)

        rospy.loginfo("l_Waiting for move_base action server...")
        self.l_move_base_client.wait_for_server()

        self.l_pose_stamped_arr = []
        self.l_pose_array = PoseArray()
        self.l_pose_array.header.frame_id = 'map'
        self.l_next_pose_index = 0

        self.l_point_list = []
        self.l_path = Path()

        self.already_back_to_first = False

        self.canceled_point = []

        self.current_goal = None  # set this to cancel goal

    def handle_2d_nav_goal_clicked(self, pose_stamped):
        self.l_pose_stamped_arr.append(pose_stamped)
        self.mark_point(pose_stamped, len(self.l_pose_stamped_arr))
        self.append_point_to_table(pose_stamped.pose)

    def handle_goal_status(self, msg):
        # 此方法一启动就会不停触发
        if len(msg.status_list) > 0:
            status = msg.status_list[-1]
            rospy.loginfo("Navigation status: {}".format(status))

    def handle_goal_result(self, msg):
        print("handle_goal_result", msg)
        self.current_goal = None

        result = msg.status.status
        if result == GoalStatus.SUCCEEDED:  # 成功到达目标
            if self.already_back_to_first is True:
                self.l_nav_clear_up()
                return
            prev_index = self.l_next_pose_index - 1
            msg = u"" if prev_index >= len(self.l_point_list) else self.l_point_list[prev_index]["message"]
            msg = msg if msg is not None and msg != u'' else u"已经到达导览点 %d" % (prev_index + 1)
            self.message = msg
            rospy.sleep(1)
            self.l_try_nav_to_next_pose()
        else:
            rospy.loginfo("Navigation result: Failed to reach goal.")

    def delete_point(self, index):
        del self.l_pose_stamped_arr[index]
        marker_delete = Marker()
        marker_delete.action = Marker.DELETEALL
        self.l_marker_pub_.publish(marker_delete)
        for i, pose_s in enumerate(self.l_pose_stamped_arr):
            self.mark_point(pose_s, i + 1)

        self.delete_path()

    def delete_path(self):
        del self.l_path.poses[:]
        self.l_path.header.frame_id = 'map'
        self.l_path.header.stamp = rospy.Time.now()
        self.m_global_path_pub_.publish(self.l_path)

    def parse_nav_path(self, point_list):
        self.l_point_list = point_list
        if len(self.l_pose_stamped_arr) == 0:
            return

        self.l_next_pose_index = 0
        del self.l_path.poses[:]
        del self.canceled_point[:]
        for index, pose in enumerate(self.l_pose_stamped_arr):
            start_pose = self.l_pose_stamped_arr[index].pose
            next_index = 0 if index == len(self.l_pose_stamped_arr) - 1 else index + 1
            goal_pose = self.l_pose_stamped_arr[next_index].pose

            request = GetPlanRequest()

            request.start.header.frame_id = "map"
            request.start.header.stamp = rospy.Time.now()
            request.start.pose = start_pose

            request.goal.header.frame_id = "map"
            request.goal.header.stamp = rospy.Time.now()
            request.goal.pose = goal_pose

            resp = self.make_plan_client(request)
            rospy.loginfo("l plan: %d to %d, poses len: %d" % (index, next_index, len(resp.plan.poses)))
            for i in resp.plan.poses:
                self.l_path.poses.append(i)

        self.l_path.header.frame_id = 'map'
        self.l_path.header.stamp = rospy.Time.now()
        self.m_global_path_pub_.publish(self.l_path)
        self.message = "路径规划完毕，随时可以出发"

    def start_nav(self, point_list):
        if len(self.l_pose_stamped_arr) == 0:
            return
        self.l_point_list = point_list
        self.already_back_to_first = False
        self.l_try_nav_to_next_pose()
        self.set_start_btn_disabled(True)

    def stop_nav(self):
        rospy.loginfo("Button stop pressed.")
        # if self.l_next_pose_index != 0:
        self.cancel_goal()
            # return

    def cancel_goal(self):
        # if self.send_goal:
        #     self.l_move_base_client.cancel_goal()
        #     self.set_stop_btn_disabled(True)
        #     self.set_start_btn_disabled(False)

        if self.current_goal is not None:
            self.l_goal_cancel_pub_.publish(self.current_goal.goal_id)

            self.set_stop_btn_disabled(True)
            self.set_start_btn_disabled(False)

            self.message = "已经取消导航到目标点 %d" % self.l_next_pose_index
            self.canceled_point.append(self.l_next_pose_index)
            if self.l_next_pose_index >= len(self.l_pose_stamped_arr):
                self.l_nav_clear_up()
            self.current_goal = None

    def append_point_to_table(self, pose):
        rospy.loginfo("append to table: x: %f, y: %f, z: %f" % (pose.position.x, pose.position.y, pose.position.z))
        self.append_row(["x: %.2f, y: %.2f" % (pose.position.x, pose.position.y), ""])
        pass

    def mark_point(self, pose_stamped, index):
        arrow = Marker()
        number = Marker()
        arrow.header.frame_id = number.header.frame_id = pose_stamped.header.frame_id
        arrow.ns = "navi_point_arrow"
        number.ns = "navi_point_number"
        arrow.action = number.action = Marker.ADD
        arrow.type = Marker.ARROW
        number.type = Marker.TEXT_VIEW_FACING
        arrow.pose = number.pose = pose_stamped.pose
        number.pose.position.z = 1.0
        arrow.scale.x = 1.0
        arrow.scale.y = 0.2
        number.scale.z = 1.0
        arrow.color.r = number.color.r = 1.0
        arrow.color.g = number.color.g = 0.98
        arrow.color.b = number.color.b = 0.80
        arrow.color.a = number.color.a = 1.0
        arrow.id = number.id = index
        number.text = str(index)

        self.l_marker_pub_.publish(number)
        rospy.sleep(0.08)
        self.l_marker_pub_.publish(arrow)
        rospy.sleep(0.02)

        rospy.loginfo("point and arrow published: %s" % number.text)

    def message_thread(self):
        if self.message is not None:
            self.try_append_line(self.message)
            self.message = None

    def try_append_line(self, t):
        text = t if isinstance(t, unicode) else unicode(t, "utf-8")
        self.last_message = self.last_message if isinstance(self.last_message, unicode) else unicode(self.last_message,
                                                                                                     "utf-8")
        if self.last_message != text:
            self.last_message = text
            self.plainTextEdit.appendPlainText("robot: %s" % text)

    def l_nav_clear_up(self):
        rospy.loginfo("All navigation point has executed, now delete all navigation marker text and arrow.")
        self.message = "已经完成本次导览任务"
        self.l_next_pose_index = 0
        self.set_stop_btn_disabled(True)
        self.set_start_btn_disabled(False)

    def l_next_pose_index_text(self):
        return self.l_next_pose_index + 1

    def increase_next_index(self):
        next_index = self.l_next_pose_index + 1
        self.l_next_pose_index = next_index

    def l_try_nav_to_next_pose(self):
        if self.l_next_pose_index >= len(self.l_pose_stamped_arr):
            if not self.already_back_to_first:
                self.l_nav_to_first_pose()
                self.already_back_to_first = True
            else:
                self.l_nav_clear_up()
            return

        # rospy.loginfo("Navigating to %d." % (self.l_next_pose_index + 1))
        self.message = "正在导航到导览点 %d" % (self.l_next_pose_index_text())
        rospy.sleep(1)
        # goal = MoveBaseGoal()
        # goal.target_pose.header.frame_id = "map"
        # goal.target_pose.pose = self.l_pose_stamped_arr[self.l_next_pose_index].pose

        # self.l_move_base_client.send_goal(goal, done_cb=self.l_done_cb)

        goal = MoveBaseActionGoal()
        goal.header.frame_id = "map"
        goal.header.stamp = rospy.Time.now()
        goal.goal_id.id = self.goal_id()
        goal.goal.target_pose.pose = self.l_pose_stamped_arr[self.l_next_pose_index].pose
        goal.goal.target_pose.header.frame_id = "map"
        self.l_goal_pub_.publish(goal)
        self.current_goal = goal

        self.increase_next_index()

    def goal_id(self):
        return "demo_goal_%d_%s" % (self.l_next_pose_index, rospy.Time.now().to_sec())

    def l_nav_to_first_pose(self):
        cancel_msg = "（已取消的点%s除外）" % ', '.join(map(lambda p: str(p), self.canceled_point)) if len(
            self.canceled_point) > 0 else ""
        self.message = "已经到达过所有导览点%s，正在返回导览点 1" % cancel_msg
        rospy.sleep(0.5)
        # goal = MoveBaseGoal()
        # goal.target_pose.header.frame_id = "map"
        # goal.target_pose.pose = self.l_pose_stamped_arr[0].pose
        # self.l_move_base_client.send_goal(goal, done_cb=self.l_done_cb)

        goal = MoveBaseActionGoal()
        goal.header.frame_id = "map"
        goal.header.stamp = rospy.Time.now()
        goal.goal_id.id = "demo_goal_0_%s" % rospy.Time.now().to_sec()
        goal.goal.target_pose.pose = self.l_pose_stamped_arr[0].pose
        goal.goal.target_pose.header.frame_id = "map"
        self.l_goal_pub_.publish(goal)
        self.current_goal = goal

        del self.canceled_point[:]

    def l_done_cb(self, state, result):
        self.current_goal = None
        if state == GoalStatus.PREEMPTED:
            # rospy.loginfo("Navigating to %d canceled, result: %s" % (self.l_next_pose_index, result))
            self.message = "已经取消导航到目标点 %d" % self.l_next_pose_index
            self.canceled_point.append(self.l_next_pose_index)
            if self.l_next_pose_index >= len(self.l_pose_stamped_arr):
                self.l_nav_clear_up()
                return

        if state == GoalStatus.SUCCEEDED:
            if self.already_back_to_first is True:
                self.l_nav_clear_up()
                return
            # rospy.loginfo(self.l_next_pose_index)
            prev_index = self.l_next_pose_index - 1
            msg = u"" if len(self.l_point_list) == 0 else self.l_point_list[prev_index]["message"]
            msg = msg if msg is not None and msg != u'' else u"已经到达导览点 %d" % (prev_index + 1)
            self.message = msg
            rospy.sleep(1)
            self.l_try_nav_to_next_pose()

    def delete_marker(self):
        marker_delete = Marker()
        marker_delete.action = Marker.DELETEALL
        self.l_marker_pub_.publish(marker_delete)
        del self.l_pose_stamped_arr[:]
